/*
首先，订阅两只乌龟的坐标系信息，
然后，将乌龟1的位置转换到乌龟2的坐标系下面，
接着，计算乌龟2的运动线速度和角速度，
最后，控制乌龟2运动。

*/

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/Twist.h"
#include <cmath>

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "control_turtle");
    ros::NodeHandle nh;
    ros::Rate rate(10);

    //订阅两只乌龟的信息
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener lister(buffer);
    ros::Publisher pub_control = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);
    while (ros::ok())
    {
        rate.sleep();
        try
        {
            //坐标转换
            geometry_msgs::TransformStamped t1ATt2 =
                buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
            // ROS_INFO("t1 在 t2 的相对位置为:(%.2f,%.2f,%.2f)",
            //          t1ATt2.transform.translation.x,
            //          t1ATt2.transform.translation.y,
            //          t1ATt2.transform.translation.z);
            // tf2::Quaternion qtn(t1ATt2.transform.rotation.x,
            //                     t1ATt2.transform.rotation.y,
            //                     t1ATt2.transform.rotation.z,
            //                     t1ATt2.transform.rotation.w);
            // ROS_INFO("rotation z:%.2f.", qtn.getAngle());

            //计算turtle2运动速度
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(t1ATt2.transform.translation.x, 2) + pow(t1ATt2.transform.translation.y, 2));
            twist.angular.z = 4 * atan2(t1ATt2.transform.translation.y, t1ATt2.transform.translation.x);

            //创建发布话题，控制乌龟运动
            pub_control.publish(twist);
        }
        catch (const std::exception &e)
        {
            ROS_INFO("错误提示:%s", e.what());
        }
        
        ros::spinOnce();
    }
    return 0;
}
